﻿using System;
using System.Drawing;
using OpenCvSharp;
using System.Xml;

namespace GeoSketch2.Core
{
    /// <summary>
    /// A class which contains some useful geometric transformations
    /// </summary>
    public class Transformations
    {
        /// <summary>
        /// A 2D Helmert transformation function
        /// </summary>
        public class HelmertTransformation2D
        {
            #region Fields
            private double a, inverse_a;
            private double b, inverse_b;
            private double c, inverse_c;
            private double d, inverse_d; 
            #endregion

            #region Constructor
            /// <summary>
            /// A 2D Helmert transformation to compute coordinates from one planimetric coordinates system to another
            /// </summary>
            public HelmertTransformation2D()
            {
                Reset();
            } 
            #endregion

            #region Methods
            #region Estimation of the elements of the transformation
            /// <summary>
            /// Computes the element of the transformation from multiple pairs of points
            /// </summary>
            /// <param name="SourcePoints">Points from the original coordinates system</param>
            /// <param name="DestinationPoints">Points from the destination coordinates system</param>
            public void ComputeTranformationFromPoints(Point2D32S[] SourcePoints, Point2D32S[] DestinationPoints)
            {
                ComputeTranformationFromPoints(SourcePoints.To64D(), DestinationPoints.To64D());
            }

            /// <summary>
            /// Computes the element of the transformation from multiple pairs of points
            /// </summary>
            /// <param name="SourcePoints">Points from the original coordinates system</param>
            /// <param name="DestinationPoints">Points from the destination coordinates system</param>
            public void ComputeTranformationFromPoints(Point2D32F[] SourcePoints, Point2D32F[] DestinationPoints)
            {
                ComputeTranformationFromPoints(SourcePoints.To64D(), DestinationPoints.To64D());
            }

            /// <summary>
            /// Computes the element of the transformation from multiple pairs of points
            /// </summary>
            /// <param name="SourcePoints">Points from the original coordinates system</param>
            /// <param name="DestinationPoints">Points from the destination coordinates system</param>
            public void ComputeTranformationFromPoints(Point2D64D[] SourcePoints, Point2D64D[] DestinationPoints)
            {
                ComputeTranformationFromPoints(SourcePoints, DestinationPoints, out a, out b, out c, out d);
                ComputeTranformationFromPoints(DestinationPoints, SourcePoints, out inverse_a, out inverse_b, out inverse_c, out inverse_d);
            }

            private void ComputeTranformationFromPoints(Point2D64D[] SourcePoints, Point2D64D[] DestinationPoints, out double _a, out double _b, out double _c, out double _d)
            {
                CvMat mat_A = Cv.CreateMat(SourcePoints.Length * 2, 4, MatrixType.F64C1);
                CvMat mat_B = Cv.CreateMat(SourcePoints.Length * 2, 1, MatrixType.F64C1);
                CvMat mat_X = Cv.CreateMat(4, 1, MatrixType.F64C1);

                for (int i = 0; i < SourcePoints.Length; i++)
                {
                    Cv.mSet(mat_A, 2 * i, 0, SourcePoints[i].X);
                    Cv.mSet(mat_A, 2 * i, 1, -SourcePoints[i].Y);
                    Cv.mSet(mat_A, 2 * i, 2, 1);
                    Cv.mSet(mat_A, 2 * i, 3, 0);
                    Cv.mSet(mat_B, 2 * i, 0, DestinationPoints[i].X);

                    Cv.mSet(mat_A, 2 * i + 1, 0, SourcePoints[i].Y);
                    Cv.mSet(mat_A, 2 * i + 1, 1, SourcePoints[i].X);
                    Cv.mSet(mat_A, 2 * i + 1, 2, 0);
                    Cv.mSet(mat_A, 2 * i + 1, 3, 1);
                    Cv.mSet(mat_B, 2 * i + 1, 0, DestinationPoints[i].Y);
                }

                Cv.Solve(mat_A, mat_B, mat_X, InvertMethod.Svd);

                _a = Cv.mGet(mat_X, 0, 0);
                _b = Cv.mGet(mat_X, 1, 0);
                _c = Cv.mGet(mat_X, 2, 0);
                _d = Cv.mGet(mat_X, 3, 0);

                /*_scale = Math.Sqrt(a * a + b * b);
                _angle = Math.Atan2(a, b);

                _tx = Cv.mGet(mat_X, 2, 0);
                _ty = Cv.mGet(mat_X, 3, 0);*/
            }

            /// <summary>
            /// The computation of the elements of the transformation from rectangles
            /// </summary>
            public void ComputeTranformationFromTargetArea(RectangleD SourceRectangle, RectangleD TargetRectangle)
            {
                RectangleD source_on_target = Tools.FitRectangleIntoRectangle(TargetRectangle, SourceRectangle);//The rectangle the source should be on the target rectangle
                Point2D64D[] sources_points, destination_points;

                sources_points = SourceRectangle.ToArray();

                destination_points = source_on_target.ToArray();

                ComputeTranformationFromPoints(sources_points, destination_points);
            } 
            #endregion

            #region Coordinates transformation
            /// <summary>
            /// Transforms the coordinates of a point from the original coordinates system to the destination one
            /// </summary>
            /// <param name="Point">The point to transform</param>
            public Point2D64D Transform(Point2D64D Point)
            {
                return new Point2D64D(
                    c + a * Point.X - b * Point.Y,
                    d + b * Point.X + a * Point.Y
                    );
            }

            /// <summary>
            /// Transforms the coordinates of a point from the destination coordinates system to the original one
            /// </summary>
            /// <param name="Point">The point to transform</param>
            public Point2D64D InverseTransform(Point2D64D Point)
            {
                return new Point2D64D(
                    inverse_c + inverse_a * Point.X - inverse_b * Point.Y,
                    inverse_d + inverse_b * Point.X + inverse_a * Point.Y
                    );
            }

            /// <summary>
            /// Transforms the coordinates of an array of points from the original coordinates system to the destination one
            /// </summary>
            /// <param name="Points">The points to transform</param>
            public Point2D64D[] Transform(Point2D64D[] Points)
            {
                Point2D64D[] dst = new Point2D64D[Points.Length];
                for (int i = 0; i < Points.Length; i++)
                {
                    dst[i] = Transform(Points[i]);
                }
                return dst;
            }

            /// <summary>
            /// Transforms the coordinates of an array of points from the destination coordinates system to the original one
            /// </summary>
            /// <param name="Points">The points to transform</param>
            public Point2D64D[] InverseTransform(Point2D64D[] Points)
            {
                Point2D64D[] dst = new Point2D64D[Points.Length];
                for (int i = 0; i < Points.Length; i++)
                {
                    dst[i] = InverseTransform(Points[i]);
                }
                return dst;
            }
            #endregion

            /// <summary>
            /// Reset the transformation
            /// </summary>
            public void Reset()
            {
                a = 0; inverse_a = 0;
                b = 0; inverse_b = 0;
                c = 0; inverse_c = 0;
                d = 0; inverse_d = 0;
            }
            #endregion

            #region Properties
            /// <summary>
            /// Gets the angle of the transformation (radians)
            /// </summary>
            public double Angle
            {
                get
                {
                    return Math.Atan2(b, a);
                }
            }

            /// <summary>
            /// Gets the scale of the transformation
            /// </summary>
            public double Scale
            {
                get
                {
                    return Math.Sqrt(a * a + b * b);
                }
            }

            /// <summary>
            /// Gets the X translation of the transformation
            /// </summary>
            public double Tx
            {
                get
                {
                    return c;
                }
            }

            /// <summary>
            /// Gets the Y of the transformation
            /// </summary>
            public double Ty
            {
                get
                {
                    return d;
                }
            }

            /// <summary>
            /// Gets the angle of the inverse transformation (radians)
            /// </summary>
            public double InverseAngle
            {
                get
                {
                    return Math.Atan2(inverse_b, inverse_a);
                }
            }

            /// <summary>
            /// Gets the scale of the inverse transformation
            /// </summary>
            public double InverseScale
            {
                get
                {
                    return Math.Sqrt(inverse_a * inverse_a + inverse_b * inverse_b);
                }
            }

            /// <summary>
            /// Gets the X translation of the inverse transformation
            /// </summary>
            public double InverseTx
            {
                get
                {
                    return inverse_c;
                }
            }

            /// <summary>
            /// Gets the Y of the inverse transformation
            /// </summary>
            public double InverseTy
            {
                get
                {
                    return inverse_d;
                }
            } 
            #endregion
        }

        /// <summary>
        /// A 2D affine transformation function
        /// X=ax+by+c
        /// Y=dx+ey+f
        /// </summary>
        public class AffineTransformation2D
        {
            #region XMLElementsName
#pragma warning disable 1591
            public const string XML_a = "a";
            public const string XML_b = "b";
            public const string XML_c = "c";
            public const string XML_d = "d";
            public const string XML_e = "e";
            public const string XML_f = "f";
            public const string XML_inverse_a = "inverse_a";
            public const string XML_inverse_b = "inverse_b";
            public const string XML_inverse_c = "inverse_c";
            public const string XML_inverse_d = "inverse_d";
            public const string XML_inverse_e = "inverse_e";
            public const string XML_inverse_f = "inverse_f";
#pragma warning restore 1591
            #endregion

            #region Fields
            private double a, inverse_a;
            private double b, inverse_b;
            private double c, inverse_c;
            private double d, inverse_d;
            private double e, inverse_e;
            private double f, inverse_f;
            #endregion

            #region Constructor
            /// <summary>
            /// A 2D affine transformation to compute coordinates from one planimetric coordinates system to another
            /// </summary>
            public AffineTransformation2D()
            {
                Reset();
            }
            #endregion

            #region Methods
            #region Estimation of the elements of the transformation
            /// <summary>
            /// Computes the element of the transformation from multiple pairs of points
            /// </summary>
            /// <param name="SourcePoints">Points from the original coordinates system</param>
            /// <param name="DestinationPoints">Points from the destination coordinates system</param>
            public void ComputeTranformationFromPoints(Point2D32S[] SourcePoints, Point2D32S[] DestinationPoints)
            {
                ComputeTranformationFromPoints(SourcePoints.To64D(), DestinationPoints.To64D());
            }

            /// <summary>
            /// Computes the element of the transformation from multiple pairs of points
            /// </summary>
            /// <param name="SourcePoints">Points from the original coordinates system</param>
            /// <param name="DestinationPoints">Points from the destination coordinates system</param>
            public void ComputeTranformationFromPoints(Point2D32F[] SourcePoints, Point2D32F[] DestinationPoints)
            {
                ComputeTranformationFromPoints(SourcePoints.To64D(), DestinationPoints.To64D());
            }

            /// <summary>
            /// Computes the element of the transformation from multiple pairs of points
            /// </summary>
            /// <param name="SourcePoints">Points from the original coordinates system</param>
            /// <param name="DestinationPoints">Points from the destination coordinates system</param>
            public void ComputeTranformationFromPoints(Point2D64D[] SourcePoints, Point2D64D[] DestinationPoints)
            {
                ComputeTranformationFromPoints(SourcePoints, DestinationPoints, out a, out b, out c, out d,out e, out f);
                ComputeTranformationFromPoints(DestinationPoints, SourcePoints, out inverse_a, out inverse_b, out inverse_c, out inverse_d, out inverse_e, out inverse_f);
            }

            private void ComputeTranformationFromPoints(Point2D64D[] SourcePoints, Point2D64D[] DestinationPoints, out double _a, out double _b, out double _c, out double _d, out double _e, out double _f)
            {
                CvMat mat_A = Cv.CreateMat(SourcePoints.Length * 2, 6, MatrixType.F64C1);
                CvMat mat_B = Cv.CreateMat(SourcePoints.Length * 2, 1, MatrixType.F64C1);
                CvMat mat_X = Cv.CreateMat(6, 1, MatrixType.F64C1);

                for (int i = 0; i < SourcePoints.Length; i++)
                {
                    Cv.mSet(mat_A, 2 * i, 0, SourcePoints[i].X);
                    Cv.mSet(mat_A, 2 * i, 1, SourcePoints[i].Y);
                    Cv.mSet(mat_A, 2 * i, 2, 1);
                    Cv.mSet(mat_A, 2 * i, 3, 0);
                    Cv.mSet(mat_A, 2 * i, 4, 0);
                    Cv.mSet(mat_A, 2 * i, 5, 0);
                    Cv.mSet(mat_B, 2 * i, 0, DestinationPoints[i].X);

                    Cv.mSet(mat_A, 2 * i + 1, 0, 0);
                    Cv.mSet(mat_A, 2 * i + 1, 1, 0);
                    Cv.mSet(mat_A, 2 * i + 1, 2, 0);
                    Cv.mSet(mat_A, 2 * i + 1, 3, SourcePoints[i].X);
                    Cv.mSet(mat_A, 2 * i + 1, 4, SourcePoints[i].Y);
                    Cv.mSet(mat_A, 2 * i + 1, 5, 1);
                    Cv.mSet(mat_B, 2 * i + 1, 0, DestinationPoints[i].Y);
                }

                Cv.Solve(mat_A, mat_B, mat_X, InvertMethod.Svd);

                _a = Cv.mGet(mat_X, 0, 0);
                _b = Cv.mGet(mat_X, 1, 0);
                _c = Cv.mGet(mat_X, 2, 0);
                _d = Cv.mGet(mat_X, 3, 0);
                _e = Cv.mGet(mat_X, 4, 0);
                _f = Cv.mGet(mat_X, 5, 0);
            }

            /// <summary>
            /// The computation of the elements of the transformation from rectangles
            /// </summary>
            public void ComputeTranformationFromTargetArea(RectangleD SourceRectangle, RectangleD TargetRectangle)
            {
                RectangleD source_on_target = Tools.FitRectangleIntoRectangle(TargetRectangle,SourceRectangle);//The rectangle the source should be on the target rectangle
                Point2D64D[] sources_points, destination_points;

                sources_points = SourceRectangle.ToArray();

                destination_points = source_on_target.ToArray();

                ComputeTranformationFromPoints(sources_points, destination_points);
            }
            #endregion

            #region Coordinates transformation
            /// <summary>
            /// Transforms the coordinates of a point from the original coordinates system to the destination one
            /// </summary>
            /// <param name="Point">The point to transform</param>
            public Point2D64D Transform(Point2D64D Point)
            {
                return new Point2D64D(
                    a * Point.X + b * Point.Y + c,
                    d * Point.X + e * Point.Y + f
                    );
            }

            /// <summary>
            /// Transforms the coordinates of a point from the destination coordinates system to the original one
            /// </summary>
            /// <param name="Point">The point to transform</param>
            public Point2D64D InverseTransform(Point2D64D Point)
            {
                return new Point2D64D(
                    inverse_a * Point.X + inverse_b * Point.Y + inverse_c,
                    inverse_d * Point.X + inverse_e * Point.Y + inverse_f
                    );
            }

            /// <summary>
            /// Transforms the coordinates of an array of points from the original coordinates system to the destination one
            /// </summary>
            /// <param name="Points">The points to transform</param>
            public PointF[] TransformF(Point2D64D[] Points)
            {
                PointF[] dst = new PointF[Points.Length];
                for (int i = 0; i < Points.Length; i++)
                {
                    dst[i] = Transform(Points[i]);
                }
                return dst;
            }

            /// <summary>
            /// Transforms the coordinates of an array of points from the destination coordinates system to the original one
            /// </summary>
            /// <param name="Points">The points to transform</param>
            public PointF[] InverseTransformF(Point2D64D[] Points)
            {
                PointF[] dst = new PointF[Points.Length];
                for (int i = 0; i < Points.Length; i++)
                {
                    dst[i] = InverseTransform(Points[i]);
                }
                return dst;
            }

            /// <summary>
            /// Transforms the coordinates of an array of points from the original coordinates system to the destination one
            /// </summary>
            /// <param name="Points">The points to transform</param>
            public Point2D64D[] Transform(Point2D64D[] Points)
            {
                Point2D64D[] dst = new Point2D64D[Points.Length];
                for (int i = 0; i < Points.Length; i++)
                {
                    dst[i] = Transform(Points[i]);
                }
                return dst;
            }

            /// <summary>
            /// Transforms the coordinates of an array of points from the destination coordinates system to the original one
            /// </summary>
            /// <param name="Points">The points to transform</param>
            public Point2D64D[] InverseTransform(Point2D64D[] Points)
            {
                Point2D64D[] dst = new Point2D64D[Points.Length];
                for (int i = 0; i < Points.Length; i++)
                {
                    dst[i] = InverseTransform(Points[i]);
                }
                return dst;
            }

            /// <summary>
            /// Transforms the coordinates of an array of points from the original coordinates system to the destination one
            /// </summary>
            /// <param name="Rectangle">The rectangle to transform</param>
            /// <param name="YDirection">The direction of the Y axis of the output rectangle</param>
            public RectangleD Transform(RectangleD Rectangle, YAxisDirection YDirection)
            {
                return Tools.GetRectangleDBounds(Transform(Rectangle.ToArray()), YDirection);
            }

            /// <summary>
            /// Transforms the coordinates of an array of points from the destination coordinates system to the original one
            /// </summary>
            /// <param name="Rectangle">The rectangle to transform</param>
            /// <param name="YDirection">The direction of the Y axis of the output rectangle</param>
            public RectangleD InverseTransform(RectangleD Rectangle, YAxisDirection YDirection)
            {
                return Tools.GetRectangleDBounds(InverseTransform(Rectangle.ToArray()), YDirection);
            }

            /// <summary>
            /// Transforms a distance from the original coordinates system to the destination one
            /// </summary>
            /// <param name="Distance">The distance to transform</param>
            public double Transform(double Distance)
            {
                Point2D64D Origin = new Point2D64D(0, 0);
                Point2D64D Dist = new Point2D64D(Distance, 0);
                Point2D64D[] result = Transform(new Point2D64D[] { Origin, Dist });
                return GeoMath.Distance2D(result[0], result[1]);
            }

            /// <summary>
            /// Transforms a distance from the destination coordinates system to the original one
            /// </summary>
            /// <param name="Distance">The distance to transform</param>
            public double InverseTransform(double Distance)
            {
                Point2D64D Origin = new Point2D64D(0, 0);
                Point2D64D Dist = new Point2D64D(Distance, 0);
                Point2D64D[] result = InverseTransform(new Point2D64D[] { Origin, Dist });
                return GeoMath.Distance2D(result[0], result[1]);
            }
            #endregion

            #region Misc
            /// <summary>
            /// Reset the transformation
            /// </summary>
            public void Reset()
            {
                a = 0; inverse_a = 0;
                b = 0; inverse_b = 0;
                c = 0; inverse_c = 0;
                d = 0; inverse_d = 0;
                e = 0; inverse_e = 0;
                f = 0; inverse_f = 0;
            } 
            #endregion

            #region FileIO
            /// <summary>
            /// Writes the object to a file
            /// </summary>
            public void ToFile(XmlWriter Xw)
            {
                Xw.WriteStartElement(this.GetType().Name);

                #region Normal Transformation
                Xw.WriteStartElement(XML_a);
                Xw.WriteString(a.ToString(Vars.Setup.GeoSketch.NumberFormat));
                Xw.WriteEndElement();
                Xw.WriteStartElement(XML_b);
                Xw.WriteString(b.ToString(Vars.Setup.GeoSketch.NumberFormat));
                Xw.WriteEndElement();
                Xw.WriteStartElement(XML_c);
                Xw.WriteString(c.ToString(Vars.Setup.GeoSketch.NumberFormat));
                Xw.WriteEndElement();
                Xw.WriteStartElement(XML_d);
                Xw.WriteString(d.ToString(Vars.Setup.GeoSketch.NumberFormat));
                Xw.WriteEndElement();
                Xw.WriteStartElement(XML_e);
                Xw.WriteString(e.ToString(Vars.Setup.GeoSketch.NumberFormat));
                Xw.WriteEndElement();
                Xw.WriteStartElement(XML_f);
                Xw.WriteString(f.ToString(Vars.Setup.GeoSketch.NumberFormat));
                Xw.WriteEndElement();
                #endregion

                #region Inverse Transformation
                Xw.WriteStartElement(XML_inverse_a);
                Xw.WriteString(inverse_a.ToString(Vars.Setup.GeoSketch.NumberFormat));
                Xw.WriteEndElement();
                Xw.WriteStartElement(XML_inverse_b);
                Xw.WriteString(inverse_b.ToString(Vars.Setup.GeoSketch.NumberFormat));
                Xw.WriteEndElement();
                Xw.WriteStartElement(XML_inverse_c);
                Xw.WriteString(inverse_c.ToString(Vars.Setup.GeoSketch.NumberFormat));
                Xw.WriteEndElement();
                Xw.WriteStartElement(XML_inverse_d);
                Xw.WriteString(inverse_d.ToString(Vars.Setup.GeoSketch.NumberFormat));
                Xw.WriteEndElement();
                Xw.WriteStartElement(XML_inverse_e);
                Xw.WriteString(inverse_e.ToString(Vars.Setup.GeoSketch.NumberFormat));
                Xw.WriteEndElement();
                Xw.WriteStartElement(XML_inverse_f);
                Xw.WriteString(inverse_f.ToString(Vars.Setup.GeoSketch.NumberFormat));
                Xw.WriteEndElement();
                #endregion

                Xw.WriteEndElement();
            } 
            #endregion
            #endregion

            #region Properties
            
            #endregion
        }
    }
}
